'''基于final2，修改reward以优化无人机轨迹'''
import torch
import torch.nn as nn
import torch.optim as optim
import numpy as np
import matplotlib.pyplot as plt
import random
from collections import deque
import os
import time
import matplotlib

# 设置随机种子确保结果可复现
SEED = 45
torch.manual_seed(SEED)
torch.cuda.manual_seed(SEED)
np.random.seed(SEED)
random.seed(SEED)

# 检查GPU可用性
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
print(f"Using device: {device}")
plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False

EPISODES_PER_TASK = 1500

# 环境参数
AREA_SIZE = 100  # 区域大小 100m x 100m
NUM_USERS = 10  # 用户数量
MAX_STEPS = 200  # 每个episode的最大步数
MAX_DISTANCE_COLLECT = 15  # UAV可收集任务的最大距离

# UAV参数
UAV_SPEED = 15.0  # UAV速度 (m/s)
UAV_ENERGY_PER_METER = 0.1  # 每米能耗
UAV_HOVER_ENERGY = 0.5  # 悬停能耗

# 任务参数
TASK_SIZE = [10, 50]  # 任务大小范围 (MB)

# TD3参数
ACTOR_LR = 3e-4
CRITIC_LR = 3e-4
GAMMA = 0.99
TAU = 0.005
BUFFER_SIZE = 200000
BATCH_SIZE = 256
EXPLORATION_NOISE_START = 0.4
EXPLORATION_NOISE_END = 0.1  # 提高最小探索率从0.05到0.1
REWARD_SCALE = 0.1

# EWC参数
EWC_LAMBDA = 100  # EWC正则化强度
FISHER_SAMPLE_SIZE = 1000  # 计算Fisher信息矩阵的样本数

# GRU参数
SEQUENCE_LENGTH = 10  # GRU序列长度
HIDDEN_SIZE = 128  # GRU隐藏层大小


class Environment:
    def __init__(self):
        # 初始化用户位置 (固定)
        self.user_positions = np.random.uniform(0, AREA_SIZE, size=(NUM_USERS, 2))

        # 初始化任务大小 (固定)
        self.task_sizes = np.random.uniform(TASK_SIZE[0], TASK_SIZE[1], size=NUM_USERS)

        # 任务生成状态 - 初始所有用户都生成任务
        self.task_generating_users = np.ones(NUM_USERS, dtype=bool)

        # UAV初始位置
        self.uav_position = np.array([AREA_SIZE / 2, AREA_SIZE / 2], dtype=float)

        # 任务收集状态
        self.collected_tasks = np.zeros(NUM_USERS, dtype=bool)

        # 步数计数器
        self.step_count = 0

        # 总延迟和能耗
        self.total_delay = 0
        self.total_energy = 0

        # 历史轨迹
        self.trajectory = [self.uav_position.copy()]

        # 上一次的距离，用于计算奖励
        self.last_distances = np.array([np.linalg.norm(self.uav_position - pos) for pos in self.user_positions])

        # 存储观测历史用于GRU
        self.observation_history = deque(maxlen=SEQUENCE_LENGTH)

        # 当前环境阶段
        self.current_phase = 1

    def update_task_generating_users(self, phase):
        """根据训练阶段更新生成任务的用户"""
        self.current_phase = phase

        if phase == 1:  # 第一阶段：所有用户生成任务
            self.task_generating_users = np.ones(NUM_USERS, dtype=bool)
        elif phase == 2:  # 第二阶段：随机9个用户生成任务
            indices = np.random.choice(NUM_USERS, 9, replace=False)
            self.task_generating_users = np.zeros(NUM_USERS, dtype=bool)
            self.task_generating_users[indices] = True
        else:  # 第三阶段：随机8个用户生成任务
            indices = np.random.choice(NUM_USERS, 8, replace=False)
            self.task_generating_users = np.zeros(NUM_USERS, dtype=bool)
            self.task_generating_users[indices] = True

        print(f"Phase {phase}: {sum(self.task_generating_users)} users are generating tasks")
        print(f"Task generating users: {np.where(self.task_generating_users)[0]}")

    def reset(self):
        # 重置UAV位置到中心点
        self.uav_position = np.array([AREA_SIZE / 2, AREA_SIZE / 2], dtype=float)

        # 重置任务收集状态
        self.collected_tasks = np.zeros(NUM_USERS, dtype=bool)

        # 重置步数
        self.step_count = 0

        # 重置总延迟和能耗
        self.total_delay = 0
        self.total_energy = 0

        # 重置轨迹
        self.trajectory = [self.uav_position.copy()]

        # 重置上一次的距离
        self.last_distances = np.array([np.linalg.norm(self.uav_position - pos) for pos in self.user_positions])

        # 重置观测历史
        self.observation_history = deque(maxlen=SEQUENCE_LENGTH)

        # 初始状态
        state = self._get_state()

        # 填充观测历史
        for _ in range(SEQUENCE_LENGTH):
            self.observation_history.append(state)

        return self._get_gru_state()

    def step(self, action):
        # 更新UAV位置 (action是相对移动，范围[-1,1])
        action = np.clip(action, -1, 1)
        movement = action * UAV_SPEED
        prev_position = self.uav_position.copy()
        self.uav_position += movement
        self.uav_position = np.clip(self.uav_position, 0, AREA_SIZE)

        # 记录轨迹
        self.trajectory.append(self.uav_position.copy())

        # 计算移动距离和能耗
        distance_moved = np.linalg.norm(self.uav_position - prev_position)
        energy_consumed = distance_moved * UAV_ENERGY_PER_METER

        # 计算与所有用户的新距离
        new_distances = np.array([np.linalg.norm(self.uav_position - pos) for pos in self.user_positions])

        # 收集任务
        newly_collected = 0
        collected_indices = []

        for i in range(NUM_USERS):
            # 只收集生成任务的用户的任务
            if self.task_generating_users[i] and not self.collected_tasks[i]:
                if new_distances[i] <= MAX_DISTANCE_COLLECT:
                    self.collected_tasks[i] = True
                    newly_collected += 1
                    collected_indices.append(i)

                    # 计算任务延迟
                    delay = new_distances[i] * self.task_sizes[i] / 10
                    self.total_delay += delay

                    # 悬停能耗
                    energy_consumed += UAV_HOVER_ENERGY

        # 累计总能耗
        self.total_energy += energy_consumed

        # 更新步数
        self.step_count += 1

        # 计算奖励
        reward = self._calculate_reward(newly_collected, energy_consumed, collected_indices, new_distances,
                                        self.last_distances)

        # 更新上一次的距离
        self.last_distances = new_distances

        # 判断是否结束 - 收集完所有任务生成用户的任务或达到最大步数
        total_tasks_to_collect = sum(self.task_generating_users)
        collected_required_tasks = sum(self.collected_tasks & self.task_generating_users)
        done = (self.step_count >= MAX_STEPS) or (collected_required_tasks == total_tasks_to_collect)

        # 获取当前状态
        state = self._get_state()

        # 更新观测历史
        self.observation_history.append(state)

        return self._get_gru_state(), reward, done, {
            "collected": sum(self.collected_tasks),
            "collected_required": collected_required_tasks,
            "total_required": total_tasks_to_collect,
            "energy": self.total_energy,
            "delay": self.total_delay,
            "newly_collected": newly_collected,
            "total_users": NUM_USERS
        }

    def _get_state(self):
        # 状态表示: UAV位置, 与每个用户的距离, 任务收集状态, 任务生成状态, 归一化步数
        state = np.zeros(2 + NUM_USERS * 3 + 1)

        # UAV位置 (归一化)
        state[0:2] = self.uav_position / AREA_SIZE

        # 与每个用户的距离, 任务收集状态和任务生成状态
        for i in range(NUM_USERS):
            dist = np.linalg.norm(self.uav_position - self.user_positions[i])
            idx = 2 + i * 3
            state[idx] = dist / np.sqrt(2 * AREA_SIZE ** 2)  # 归一化距离
            state[idx + 1] = float(self.collected_tasks[i])
            state[idx + 2] = float(self.task_generating_users[i])

        # 归一化步数
        state[-1] = self.step_count / MAX_STEPS

        return state

    def _get_gru_state(self):
        """返回用于GRU的序列状态"""
        # 确保观测历史已满
        while len(self.observation_history) < SEQUENCE_LENGTH:
            self.observation_history.append(self._get_state())

        return np.array(list(self.observation_history))

    def _calculate_reward(self, newly_collected, energy_consumed, collected_indices, new_distances, old_distances):
        # 基础任务收集奖励 - 只计算生成任务的用户
        collection_reward = newly_collected * 20

        # 能耗惩罚 - 降低系数从0.8到0.5
        energy_penalty = energy_consumed * 0.5

        # 任务完成进度奖励 - 针对生成任务的用户
        collected_required = sum(self.collected_tasks & self.task_generating_users)
        total_required = sum(self.task_generating_users)
        progress_reward = collected_required / total_required * 10 if total_required > 0 else 0

        # 奖励收集最后一个任务 - 增加完成激励
        if collected_required == total_required and total_required > 0:
            progress_reward += 30  # 增大完成所有任务的奖励

        # 针对剩余任务越来越少的情况提供更多激励
        if total_required > 0 and collected_required > 0:
            remaining_ratio = (total_required - collected_required) / total_required
            # 当剩余任务比例小时，给予更大的收集奖励
            if remaining_ratio < 0.5:
                collection_reward *= (1.5 + (0.5 - remaining_ratio) * 3)  # 增强稀疏任务收集激励

        # 接近未收集任务的奖励 - 动态调整权重
        proximity_reward = 0
        uncollected_tasks = 0

        for i in range(NUM_USERS):
            if self.task_generating_users[i] and not self.collected_tasks[i]:
                uncollected_tasks += 1
                # 计算距离差 - 正值表示距离减少
                dist_diff = old_distances[i] - new_distances[i]

                # 随着剩余任务减少，大幅增加接近奖励权重
                if collected_required > 0 and total_required > 0:
                    # 剩余任务越少，权重越高
                    remaining_ratio = (total_required - collected_required) / total_required
                    progress_factor = 1.0 + 2.0 * (1.0 - remaining_ratio) ** 2
                else:
                    progress_factor = 1.0

                # 距离越近，奖励增益越大
                proximity_factor = max(0, 1 - (new_distances[i] / (MAX_DISTANCE_COLLECT * 3)) ** 2)
                proximity_reward += dist_diff * 3 * proximity_factor * progress_factor  # 增强接近奖励

        # 如果没有未收集任务，不提供接近奖励
        if uncollected_tasks == 0:
            proximity_reward = 0

        # 全部完成奖励 - 收集所有生成任务的用户的任务
        completion_reward = 150 if collected_required == total_required and total_required > 0 else 0

        # 新增：重复区域惩罚 - 防止原地打转
        repetition_penalty = 0
        if len(self.trajectory) > 10:
            # 计算最近几步的平均移动距离
            recent_traj = np.array(self.trajectory[-10:])
            # 计算中心点
            center = np.mean(recent_traj, axis=0)
            # 计算到中心点的平均距离，检测是否在小区域内打转
            avg_dist_to_center = np.mean([np.linalg.norm(pos - center) for pos in recent_traj])

            if avg_dist_to_center < 5.0:  # 如果在小范围内徘徊
                repetition_penalty = 5.0 * (1.0 - avg_dist_to_center / 5.0)

        # 效率奖励 - 鼓励更快完成任务
        efficiency_reward = 0
        if collected_required == total_required and total_required > 0:
            efficiency_reward = max(0, (MAX_STEPS - self.step_count) / MAX_STEPS) * 20

        # 综合奖励
        reward = collection_reward + progress_reward + proximity_reward + completion_reward + efficiency_reward - energy_penalty - repetition_penalty

        # 奖励缩放
        reward = reward * REWARD_SCALE

        return reward

    def render(self, episode=0, clear_output=True):
        """可视化当前环境状态"""
        plt.figure(figsize=(10, 10))

        # 绘制用户位置和任务状态
        for i, pos in enumerate(self.user_positions):
            if self.task_generating_users[i]:
                # 生成任务的用户
                if self.collected_tasks[i]:
                    color = 'green'  # 已收集
                else:
                    color = 'red'  # 未收集
            else:
                # 不生成任务的用户
                color = 'gray'  # 灰色表示不生成任务

            plt.scatter(pos[0], pos[1], s=100, c=color)
            plt.annotate(f"{i + 1}", (pos[0], pos[1]), fontsize=12)

        # 绘制UAV当前位置和轨迹
        trajectory = np.array(self.trajectory)
        plt.plot(trajectory[:, 0], trajectory[:, 1], 'b-', alpha=0.5)
        plt.scatter(self.uav_position[0], self.uav_position[1], s=200, c='blue', marker='*')

        # 绘制收集范围
        circle = plt.Circle((self.uav_position[0], self.uav_position[1]),
                            MAX_DISTANCE_COLLECT, color='blue', fill=False, alpha=0.3)
        plt.gca().add_patch(circle)

        plt.xlim(0, AREA_SIZE)
        plt.ylim(0, AREA_SIZE)

        # 添加任务状态信息
        title = f"Episode {episode}, Step {self.step_count}\n"
        title += f"收集: {sum(self.collected_tasks & self.task_generating_users)}/{sum(self.task_generating_users)} 任务"
        plt.title(title)
        plt.grid(True)

        plt.savefig(f"results/step_{episode}_{self.step_count}.png")
        plt.close()


class GRUActor(nn.Module):
    def __init__(self, state_dim, action_dim, max_action):
        super(GRUActor, self).__init__()

        self.state_dim = state_dim
        self.seq_len = SEQUENCE_LENGTH
        self.hidden_size = HIDDEN_SIZE

        # GRU层
        self.gru = nn.GRU(
            input_size=state_dim,
            hidden_size=self.hidden_size,
            num_layers=1,
            batch_first=True
        )

        # 全连接层
        self.layer1 = nn.Linear(self.hidden_size, 256)
        self.layer2 = nn.Linear(256, 128)
        self.layer3 = nn.Linear(128, action_dim)

        self.max_action = max_action

        self.ln1 = nn.LayerNorm(256)
        self.ln2 = nn.LayerNorm(128)

        # 存储GRU隐藏状态
        self.hidden = None

        # 初始化网络权重
        self._init_weights()

    def _init_weights(self):
        for m in self.modules():
            if isinstance(m, nn.Linear):
                nn.init.xavier_uniform_(m.weight)
                nn.init.constant_(m.bias, 0.0)

    def forward(self, state, reset_hidden=False):
        # 输入形状: [batch_size, seq_len, state_dim]

        # 如果需要重置隐藏状态或隐藏状态不存在
        if reset_hidden or self.hidden is None:
            self.reset_hidden(state.size(0))

        # GRU处理序列
        gru_out, self.hidden = self.gru(state, self.hidden)

        # 我们只使用序列中的最后一个输出
        x = gru_out[:, -1]

        # 全连接层
        x = self.ln1(torch.relu(self.layer1(x)))
        x = self.ln2(torch.relu(self.layer2(x)))
        action = torch.tanh(self.layer3(x))

        return self.max_action * action

    def reset_hidden(self, batch_size=1):
        """重置GRU的隐藏状态"""
        self.hidden = torch.zeros(1, batch_size, self.hidden_size).to(device)


class GRUCritic(nn.Module):
    def __init__(self, state_dim, action_dim):
        super(GRUCritic, self).__init__()

        self.state_dim = state_dim
        self.seq_len = SEQUENCE_LENGTH
        self.hidden_size = HIDDEN_SIZE

        # 两个独立的GRU处理状态序列
        self.q1_gru = nn.GRU(
            input_size=state_dim,
            hidden_size=self.hidden_size,
            num_layers=1,
            batch_first=True
        )

        self.q2_gru = nn.GRU(
            input_size=state_dim,
            hidden_size=self.hidden_size,
            num_layers=1,
            batch_first=True
        )

        # Q1网络
        self.q1_layer1 = nn.Linear(self.hidden_size + action_dim, 256)
        self.q1_layer2 = nn.Linear(256, 128)
        self.q1_output = nn.Linear(128, 1)

        self.q1_ln1 = nn.LayerNorm(256)
        self.q1_ln2 = nn.LayerNorm(128)

        # Q2网络
        self.q2_layer1 = nn.Linear(self.hidden_size + action_dim, 256)
        self.q2_layer2 = nn.Linear(256, 128)
        self.q2_output = nn.Linear(128, 1)

        self.q2_ln1 = nn.LayerNorm(256)
        self.q2_ln2 = nn.LayerNorm(128)

        # 存储GRU隐藏状态
        self.q1_hidden = None
        self.q2_hidden = None

        # 初始化网络权重
        self._init_weights()

    def _init_weights(self):
        for m in self.modules():
            if isinstance(m, nn.Linear):
                nn.init.xavier_uniform_(m.weight)
                nn.init.constant_(m.bias, 0.01)

    def forward(self, state, action, reset_hidden=False):
        # 状态输入形状: [batch_size, seq_len, state_dim]
        # 动作输入形状: [batch_size, action_dim]

        # 如果需要重置隐藏状态或隐藏状态不存在
        if reset_hidden or self.q1_hidden is None or self.q2_hidden is None:
            self.reset_hidden(state.size(0))

        # GRU处理序列
        q1_gru_out, self.q1_hidden = self.q1_gru(state, self.q1_hidden)
        q2_gru_out, self.q2_hidden = self.q2_gru(state, self.q2_hidden)

        # 提取序列中的最后一个输出
        q1_state = q1_gru_out[:, -1]
        q2_state = q2_gru_out[:, -1]

        # 合并状态表示和动作
        q1_x = torch.cat([q1_state, action], dim=1)
        q2_x = torch.cat([q2_state, action], dim=1)

        # Q1网络
        q1 = self.q1_ln1(torch.relu(self.q1_layer1(q1_x)))
        q1 = self.q1_ln2(torch.relu(self.q1_layer2(q1)))
        q1 = self.q1_output(q1)

        # Q2网络
        q2 = self.q2_ln1(torch.relu(self.q2_layer1(q2_x)))
        q2 = self.q2_ln2(torch.relu(self.q2_layer2(q2)))
        q2 = self.q2_output(q2)

        return q1, q2

    def Q1(self, state, action, reset_hidden=False):
        # 用于Actor训练
        if reset_hidden or self.q1_hidden is None:
            self.reset_q1_hidden(state.size(0))

        q1_gru_out, self.q1_hidden = self.q1_gru(state, self.q1_hidden)
        q1_state = q1_gru_out[:, -1]
        q1_x = torch.cat([q1_state, action], dim=1)

        q1 = self.q1_ln1(torch.relu(self.q1_layer1(q1_x)))
        q1 = self.q1_ln2(torch.relu(self.q1_layer2(q1)))
        q1 = self.q1_output(q1)

        return q1

    def reset_hidden(self, batch_size=1):
        """重置两个GRU网络的隐藏状态"""
        self.reset_q1_hidden(batch_size)
        self.reset_q2_hidden(batch_size)

    def reset_q1_hidden(self, batch_size=1):
        self.q1_hidden = torch.zeros(1, batch_size, self.hidden_size).to(device)

    def reset_q2_hidden(self, batch_size=1):
        self.q2_hidden = torch.zeros(1, batch_size, self.hidden_size).to(device)


class ReplayBuffer:
    def __init__(self, max_size=BUFFER_SIZE):
        self.buffer = deque(maxlen=max_size)

    def add(self, state, action, reward, next_state, done):
        self.buffer.append((state, action, reward, next_state, done))

    def sample(self, batch_size):
        batch = random.sample(self.buffer, min(len(self.buffer), batch_size))
        state, action, reward, next_state, done = map(np.stack, zip(*batch))
        return state, action, reward, next_state, done

    def __len__(self):
        return len(self.buffer)


class EWC:
    """弹性权重巩固(Elastic Weight Consolidation)"""

    def __init__(self, model, fisher_sample_size=FISHER_SAMPLE_SIZE):
        self.model = model
        self.fisher_sample_size = fisher_sample_size
        self.importance = {}  # Fisher信息矩阵
        self.old_params = {}  # 上一个任务的参数
        self.fisher_diagonal = {}  # Fisher对角线

    def _calculate_fisher_info(self, replay_buffer):
        """计算Fisher信息矩阵"""
        # 初始化Fisher矩阵为0
        fisher = {}
        for name, param in self.model.named_parameters():
            if param.requires_grad:
                fisher[name] = torch.zeros_like(param).to(device)

        # 采样计算梯度
        self.model.train()  # 改为训练模式，而不是eval()模式
        samples_count = min(self.fisher_sample_size, len(replay_buffer))
        if samples_count <= 0:
            return fisher

        for _ in range(samples_count):
            # 获取随机样本
            states, actions, _, _, _ = replay_buffer.sample(1)
            states = torch.FloatTensor(states).to(device)
            actions = torch.FloatTensor(actions).to(device)

            # 前向传播
            self.model.zero_grad()

            # 根据模型类型执行不同操作
            if isinstance(self.model, GRUActor):
                # 重置隐藏状态为批次大小1
                self.model.reset_hidden(1)
                # Actor模型输出动作
                outputs = self.model(states)
                # 我们想要保持当前输出，所以损失是当前输出与自身的MSE
                loss = ((outputs - actions) ** 2).mean()
            else:
                # 重置Critic隐藏状态为批次大小1
                self.model.reset_hidden(1)
                # Critic模型输出Q值
                outputs, _ = self.model(states, actions)
                # 类似地，我们使用当前输出作为目标
                loss = outputs.mean()

            # 反向传播计算梯度
            loss.backward()

            # 累加梯度的平方
            for name, param in self.model.named_parameters():
                if param.requires_grad and param.grad is not None:
                    fisher[name] += param.grad.pow(2) / samples_count

        return fisher

    def store_task_parameters(self, task_id, replay_buffer):
        """存储当前任务的参数和计算Fisher信息矩阵"""
        print(f"Storing parameters for task {task_id} and computing Fisher information matrix")

        # 存储当前参数
        self.old_params = {}
        for name, param in self.model.named_parameters():
            if param.requires_grad:
                self.old_params[name] = param.data.clone()

        # 计算Fisher信息矩阵
        self.importance = self._calculate_fisher_info(replay_buffer)

        print(f"Stored {len(self.old_params)} parameters and computed Fisher matrices")

    def calculate_ewc_loss(self, lam=EWC_LAMBDA):
        """计算EWC正则化损失"""
        loss = 0

        if not self.old_params or not self.importance:
            return loss

        for name, param in self.model.named_parameters():
            if name in self.old_params and name in self.importance and param.requires_grad:
                # 计算与旧参数的差异，并通过Fisher信息进行加权
                loss += torch.sum(self.importance[name] * (param - self.old_params[name]).pow(2))

        return lam * loss


class TD3:
    """Twin Delayed DDPG (TD3)和弹性权重巩固(EWC)的结合"""

    def __init__(self, state_dim, action_dim, max_action):
        self.actor = GRUActor(state_dim, action_dim, max_action).to(device)
        self.actor_target = GRUActor(state_dim, action_dim, max_action).to(device)
        self.actor_target.load_state_dict(self.actor.state_dict())
        self.actor_optimizer = optim.Adam(self.actor.parameters(), lr=ACTOR_LR)

        self.critic = GRUCritic(state_dim, action_dim).to(device)
        self.critic_target = GRUCritic(state_dim, action_dim).to(device)
        self.critic_target.load_state_dict(self.critic.state_dict())
        self.critic_optimizer = optim.Adam(self.critic.parameters(), lr=CRITIC_LR)

        self.max_action = max_action
        self.memory = ReplayBuffer()

        # TD3特定参数
        self.policy_noise = 0.2 * max_action
        self.noise_clip = 0.5 * max_action
        self.policy_freq = 2
        self.total_it = 0

        # EWC相关组件
        self.ewc_actor = EWC(self.actor)
        self.ewc_critic = EWC(self.critic)
        self.current_task = 1  # 初始任务ID

        # 任务特定探索噪声
        self.task_noise = {
            1: np.linspace(EXPLORATION_NOISE_START, EXPLORATION_NOISE_END, EPISODES_PER_TASK),
            2: np.linspace(EXPLORATION_NOISE_START * 0.9, EXPLORATION_NOISE_END, EPISODES_PER_TASK),
            3: np.linspace(EXPLORATION_NOISE_START * 0.8, EXPLORATION_NOISE_END, EPISODES_PER_TASK)
        }

        # 添加探索重置机制参数
        self.no_improvement_counter = 0
        self.best_episode_reward = -float('inf')
        self.exploration_reset_threshold = 50

        # 学习率调度器
        self.actor_scheduler = optim.lr_scheduler.ReduceLROnPlateau(
            self.actor_optimizer, mode='max', factor=0.5, patience=100, verbose=True
        )
        self.critic_scheduler = optim.lr_scheduler.ReduceLROnPlateau(
            self.critic_optimizer, mode='max', factor=0.5, patience=100, verbose=True
        )

    def select_action(self, state, noise_scale=EXPLORATION_NOISE_START):
        # 确保状态是GRU所需的序列格式 [batch, seq_len, feature]
        if len(state.shape) == 2:
            state = np.expand_dims(state, 0)  # 添加batch维度

        state = torch.FloatTensor(state).to(device)

        # 显式重置隐藏状态为批次大小1
        self.actor.reset_hidden(1)

        action = self.actor(state).cpu().data.numpy().flatten()

        if noise_scale > 0:
            # 修改探索噪声机制：有方向性的探索
            if random.random() < 0.7:  # 70%概率使用定向探索
                # 找出当前最接近的未收集任务，向其偏移探索方向
                env_state = state[0, -1].cpu()  # 将张量移到CPU并获取最新状态
                uncollected_tasks = []
                for i in range(NUM_USERS):
                    # 状态格式：2 + i*3 是距离，2 + i*3 + 1 是收集状态，2 + i*3 + 2 是任务生成状态
                    task_idx = 2 + i * 3
                    is_collected = env_state[task_idx + 1].item() > 0.5
                    is_generated = env_state[task_idx + 2].item() > 0.5

                    if is_generated and not is_collected:
                        uncollected_tasks.append((i, env_state[task_idx].item()))  # (任务索引, 距离)

                if uncollected_tasks:
                    # 找到最近的未收集任务
                    nearest_task = min(uncollected_tasks, key=lambda x: x[1])
                    task_idx = nearest_task[0]

                    # 计算UAV当前位置 (使用.item()获取标量值)
                    uav_pos = np.array([env_state[0].item(), env_state[1].item()]) * AREA_SIZE

                    # 生成有偏向的噪声，更倾向于向未收集任务方向探索
                    bias = noise_scale * 1.5  # 增大偏向性
                    noise = np.random.normal(0, noise_scale, size=action.shape)

                    # 基于任务距离添加方向性偏好
                    action = action + noise
                else:
                    # 常规随机噪声
                    noise = np.random.normal(0, noise_scale, size=action.shape)
                    action = action + noise
            else:
                # 30%概率使用纯随机探索
                noise = np.random.normal(0, noise_scale * 1.2, size=action.shape)  # 增大随机噪声
                action = action + noise

        return np.clip(action, -self.max_action, self.max_action)

    def switch_task(self, task_id):
        """切换到新任务"""
        print(f"\nSwitching to task {task_id}")

        # 存储旧任务参数和计算Fisher信息矩阵
        if self.current_task > 0 and len(self.memory) > 0:
            self.ewc_actor.store_task_parameters(self.current_task, self.memory)
            self.ewc_critic.store_task_parameters(self.current_task, self.memory)

        # 更新当前任务
        self.current_task = task_id

        # 重置Actor和Critic的GRU状态
        self.actor.reset_hidden()
        self.critic.reset_hidden()

        print(f"Reset GRU states for new task {task_id}")

    def train(self):
        self.total_it += 1

        if len(self.memory) < BATCH_SIZE:
            return

        # 从经验回放中采样
        state, action, reward, next_state, done = self.memory.sample(BATCH_SIZE)

        state = torch.FloatTensor(state).to(device)
        action = torch.FloatTensor(action).to(device)
        reward = torch.FloatTensor(reward.reshape(-1, 1)).to(device)
        next_state = torch.FloatTensor(next_state).to(device)
        done = torch.FloatTensor(done.reshape(-1, 1)).to(device)

        # 重置批次中所有样本的隐藏状态
        self.critic.reset_hidden(BATCH_SIZE)
        self.critic_target.reset_hidden(BATCH_SIZE)
        self.actor.reset_hidden(BATCH_SIZE)
        self.actor_target.reset_hidden(BATCH_SIZE)

        with torch.no_grad():
            # 选择下一个动作带噪声
            noise = (torch.randn_like(action) * self.policy_noise).clamp(-self.noise_clip, self.noise_clip)
            next_action = (self.actor_target(next_state) + noise).clamp(-self.max_action, self.max_action)

            # 使用Critic目标网络计算目标Q值
            target_Q1, target_Q2 = self.critic_target(next_state, next_action)
            target_Q = torch.min(target_Q1, target_Q2)
            target_Q = reward + (1 - done) * GAMMA * target_Q

        # 获取当前Q值
        current_Q1, current_Q2 = self.critic(state, action)

        # 计算Critic损失
        critic_loss = nn.MSELoss()(current_Q1, target_Q) + nn.MSELoss()(current_Q2, target_Q)

        # 添加EWC正则化损失
        critic_ewc_loss = self.ewc_critic.calculate_ewc_loss()
        critic_loss += critic_ewc_loss

        # 更新Critic
        self.critic_optimizer.zero_grad()
        critic_loss.backward()
        torch.nn.utils.clip_grad_norm_(self.critic.parameters(), 1.0)  # 梯度裁剪
        self.critic_optimizer.step()

        # 延迟策略更新
        if self.total_it % self.policy_freq == 0:
            # 计算Actor损失
            self.actor.reset_hidden(BATCH_SIZE)
            self.critic.reset_hidden(BATCH_SIZE)
            actor_loss = -self.critic.Q1(state, self.actor(state)).mean()

            # 添加EWC正则化损失
            actor_ewc_loss = self.ewc_actor.calculate_ewc_loss()
            actor_loss += actor_ewc_loss

            # 更新Actor
            self.actor_optimizer.zero_grad()
            actor_loss.backward()
            torch.nn.utils.clip_grad_norm_(self.actor.parameters(), 1.0)  # 梯度裁剪
            self.actor_optimizer.step()

            # 软更新目标网络
            for param, target_param in zip(self.critic.parameters(), self.critic_target.parameters()):
                target_param.data.copy_(TAU * param.data + (1 - TAU) * target_param.data)

            for param, target_param in zip(self.actor.parameters(), self.actor_target.parameters()):
                target_param.data.copy_(TAU * param.data + (1 - TAU) * target_param.data)

    def update_exploration(self, episode_reward):
        """动态调整探索率，防止卡在局部最优"""
        if episode_reward > self.best_episode_reward:
            self.best_episode_reward = episode_reward
            self.no_improvement_counter = 0
        else:
            self.no_improvement_counter += 1

        # 长时间没有改善时，临时提高探索率
        if self.no_improvement_counter >= self.exploration_reset_threshold:
            # 临时增加噪声
            current_episode = self.total_it // MAX_STEPS
            phase = 1
            if current_episode > EPISODES_PER_TASK * 2:
                phase = 3
            elif current_episode > EPISODES_PER_TASK:
                phase = 2

            # 重置探索噪声
            noise_idx = current_episode % EPISODES_PER_TASK
            self.task_noise[phase][noise_idx:] = np.linspace(
                EXPLORATION_NOISE_START * 0.7,
                EXPLORATION_NOISE_END,
                EPISODES_PER_TASK - noise_idx
            )

            print(f"Reset exploration noise at episode {current_episode}, phase {phase}")
            self.no_improvement_counter = 0

    def save(self, filename):
        torch.save(self.actor.state_dict(), filename + "_actor")
        torch.save(self.critic.state_dict(), filename + "_critic")
        torch.save(self.actor_target.state_dict(), filename + "_actor_target")
        torch.save(self.critic_target.state_dict(), filename + "_critic_target")

    def load(self, filename):
        self.actor.load_state_dict(torch.load(filename + "_actor"))
        self.critic.load_state_dict(torch.load(filename + "_critic"))
        self.actor_target.load_state_dict(torch.load(filename + "_actor_target"))
        self.critic_target.load_state_dict(torch.load(filename + "_critic_target"))


def evaluate_policy(agent, env, phase=1, eval_episodes=5):
    """评估当前策略的性能"""
    avg_reward = 0.
    avg_collection_rate = 0.
    avg_energy = 0.

    # 更新环境阶段
    env.update_task_generating_users(phase)

    for _ in range(eval_episodes):
        state = env.reset()
        done = False
        while not done:
            action = agent.select_action(state, noise_scale=0)  # 不使用探索噪声
            state, reward, done, info = env.step(action)
            avg_reward += reward
            if done:
                avg_collection_rate += info['collected_required'] / info['total_required'] if info[
                                                                                                  'total_required'] > 0 else 1.0
                avg_energy += info['energy']

    avg_reward /= eval_episodes
    avg_collection_rate /= eval_episodes
    avg_energy /= eval_episodes

    print(
        f"Evaluation over {eval_episodes} episodes: {avg_reward:.3f} reward, {avg_collection_rate:.2%} collection rate, {avg_energy:.2f} energy")
    return avg_reward, avg_collection_rate, avg_energy


def train():
    # 创建结果目录
    if not os.path.exists("results"):
        os.makedirs("results")

    env = Environment()
    state_dim = 2 + NUM_USERS * 3 + 1  # UAV位置(2) + 每个用户的状态(3*NUM_USERS) + 归一化步数(1)
    action_dim = 2  # x, y移动
    max_action = 1.0

    # 初始化TD3智能体
    agent = TD3(state_dim, action_dim, max_action)

    # 保存训练数据
    training_data = {
        'rewards': [],
        'collection_rates': [],
        'energies': [],
        'eval_rewards': [],
        'eval_collection_rates': [],
        'eval_energies': []
    }

    eval_freq = 50  # 评估频率
    episodes_per_task = EPISODES_PER_TASK  # 每个任务的训练轮数

    # 初始任务阶段
    current_phase = 1

    for phase in range(1, 4):
        print(f"\n=== Starting Phase {phase} Training ===")

        # 更新环境任务生成用户
        env.update_task_generating_users(phase)

        # 通知智能体任务切换
        agent.switch_task(phase)

        for episode in range(1, episodes_per_task + 1):
            # 重置环境
            state = env.reset()
            episode_reward = 0
            episode_steps = 0
            done = False

            # 从任务特定噪声方案中获取噪声级别
            noise_scale = agent.task_noise[phase][episode - 1]

            # 收集一个episode的经验
            while not done and episode_steps < MAX_STEPS:
                # 选择动作
                action = agent.select_action(state, noise_scale)

                # 执行动作
                next_state, reward, done, info = env.step(action)

                # 存储经验
                agent.memory.add(state, action, reward, next_state, done)

                # 训练智能体
                agent.train()

                state = next_state
                episode_reward += reward
                episode_steps += 1

            # 记录训练数据
            training_data['rewards'].append(episode_reward)
            training_data['collection_rates'].append(
                info['collected_required'] / info['total_required'] if info['total_required'] > 0 else 1.0)
            training_data['energies'].append(info['energy'])

            # 更新学习率调度器
            agent.actor_scheduler.step(episode_reward)
            agent.critic_scheduler.step(episode_reward)

            # 训练结束后更新探索机制
            agent.update_exploration(episode_reward)

            # 打印训练进度
            print(f"Phase: {phase}, Episode: {episode}/{episodes_per_task}, "
                  f"Reward: {episode_reward:.2f}, Collection: {info['collected_required']}/{info['total_required']}, "
                  f"Rate: {info['collected_required'] / info['total_required'] * 100:.1f}% (Noise: {noise_scale:.3f})")

            # 定期评估
            if episode % eval_freq == 0 or episode == episodes_per_task:
                # 可视化环境
                env.render(episode)

                # 评估策略
                eval_reward, eval_collection_rate, eval_energy = evaluate_policy(agent, env, phase)
                training_data['eval_rewards'].append(eval_reward)
                training_data['eval_collection_rates'].append(eval_collection_rate)
                training_data['eval_energies'].append(eval_energy)

                # 保存模型
                agent.save(f"results/td3_gru_ewc_phase{phase}")

                # 可视化训练曲线
                plot_training_curves(training_data, phase, episode)

    print("Training completed!")


def plot_training_curves(data, phase, episode):
    plt.figure(figsize=(20, 10))

    # 绘制奖励曲线
    plt.subplot(2, 2, 1)
    plt.plot(data['rewards'], label='Training Reward')
    plt.scatter(range(0, len(data['eval_rewards']) * 50, 50), data['eval_rewards'], color='red', marker='o',
                label='Evaluation Reward')
    plt.xlabel('Episodes')
    plt.ylabel('Reward')
    plt.title('Training and Evaluation Rewards')
    plt.legend()
    plt.grid(True)

    # 绘制收集率曲线
    plt.subplot(2, 2, 2)
    plt.plot(data['collection_rates'], label='Training Collection Rate')
    plt.scatter(range(0, len(data['eval_collection_rates']) * 50, 50), data['eval_collection_rates'], color='red',
                marker='o', label='Evaluation Collection Rate')
    plt.xlabel('Episodes')
    plt.ylabel('Collection Rate')
    plt.title('Task Collection Rate')
    plt.legend()
    plt.grid(True)

    # 绘制能耗曲线
    plt.subplot(2, 2, 3)
    plt.plot(data['energies'], label='Training Energy')
    plt.scatter(range(0, len(data['eval_energies']) * 50, 50), data['eval_energies'], color='red', marker='o',
                label='Evaluation Energy')
    plt.xlabel('Episodes')
    plt.ylabel('Energy')
    plt.title('Energy Consumption')
    plt.legend()
    plt.grid(True)

    plt.tight_layout()
    plt.savefig(f"results/training_curves_phase{phase}_episode{episode}.png")
    plt.close()


def test():
    """使用训练好的模型进行测试"""
    env = Environment()
    state_dim = 2 + NUM_USERS * 3 + 1
    action_dim = 2
    max_action = 1.0

    agent = TD3(state_dim, action_dim, max_action)

    # 测试所有阶段
    for phase in range(1, 4):
        print(f"\n=== Testing Phase {phase} ===")

        # 加载对应阶段的模型
        agent.load(f"results/td3_gru_ewc_phase{phase}")

        # 更新环境任务生成用户
        env.update_task_generating_users(phase)

        # 进行测试
        state = env.reset()
        done = False
        total_reward = 0
        steps = 0

        while not done and steps < MAX_STEPS:
            # 选择动作 (无探索噪声)
            action = agent.select_action(state, noise_scale=0)

            # 执行动作
            next_state, reward, done, info = env.step(action)

            # 渲染环境
            env.render(f"test_phase{phase}", clear_output=False)

            state = next_state
            total_reward += reward
            steps += 1

        print(f"Test Phase {phase} completed:")
        print(f"Total Reward: {total_reward:.2f}")
        print(
            f"Collection Rate: {info['collected_required']}/{info['total_required']} ({info['collected_required'] / info['total_required'] * 100:.1f}%)")
        print(f"Energy Consumption: {info['energy']:.2f}")
        print(f"Total Steps: {steps}")


if __name__ == "__main__":
    train()
    test()

